package maths;

import java.util.HashMap;

import Objects.Body;


public class Runge {
	
	public HashMap<Integer, Body> bodies;
	private float step;
	private Integer time;
	int check;
	
	public Runge(HashMap<Integer, Body> bodies, Integer time){
		this.bodies=bodies;
		check=0;
		this.time=time;
		this.step = 1/100;
	}
	
	public void compute(Body b, float rx, float ry) 
	{
		float k1[]=new float[6], k2[]=new float[6], k3[]=new float[6], k4[]=new float[6];
		
		k1[0]=(b.forceX/b.mass)*time;
		k2[0]=((b.forceX/b.mass)+(step/2))*(time+(step*k1[0])/2);
		k3[0]=((b.forceX/b.mass)+(step/2))*(time+(step*k2[0])/2);
		k4[0]=((b.forceX/b.mass)+step)*(time+(step*k3[0]));
		b.shape.setCenterX(b.shape.getCenterX()+(step*(k1[0]+(2*k2[0]))+(2*k3[0])+k4[0])/6);
		b.shape.setCenterX(b.shape.getCenterX()+b.velocityX);
		
		k1[1]=(b.forceY/b.mass)*time;
		k2[1]=((b.forceY/b.mass)+(step/2))*(time+(step*k1[1])/2);
		k3[1]=((b.forceY/b.mass)+(step/2))*(time+(step*k2[1])/2);
		k4[1]=((b.forceY/b.mass)+step)*(time+(step*k3[1]));
		b.shape.setCenterY(b.shape.getCenterY()+((step*(k1[1]+(2*k2[1]))+(2*k3[1])+k4[1])/6));
		b.shape.setCenterY(b.shape.getCenterY()+b.velocityY);
		
		float force= (float)Math.sqrt(Math.pow(b.forceX,2)+Math.pow(b.forceY,2));
		float r=(float)Math.sqrt(Math.pow(rx,1)+Math.pow(ry,2));
		
		if(b.angularVelocity!=0){
			k1[2]=(force/r)*time;
			k2[2]=((force/r)+(step/2))*(time+(step*k1[2])/2);
			k3[2]=((force/r)+(step/2))*(time+(step*k2[2])/2);
			k4[2]=((force/r)+step)*(time+(step*k3[2]));
			b.angle=((step*(k1[2]+(2*k2[2]))+(2*k3[2])+k4[2])/6);	
		}
		
		k1[3]=(b.forceX/b.mass);
		k2[3]=((b.forceX/b.mass)+(step/2))*((step*k1[3])/2);
		k3[3]=((b.forceX/b.mass)+(step/2))*((step*k2[3])/2);
		k4[3]=((b.forceX/b.mass)+step)*(step*k3[3]);
		b.setVx(b.getVx()+((step*(k1[3]+(2*k2[3]))+(2*k3[3])+k4[3])/6));
		
		k1[4]=(b.forceY/b.mass);
		k2[4]=((b.forceY/b.mass)+(step/2))*((step*k1[4])/2);
		k3[4]=((b.forceY/b.mass)+(step/2))*((step*k2[4])/2);
		k4[4]=((b.forceY/b.mass)+step)*(step*k3[4]);
		b.setVy(b.getVy()+((step*(k1[4]+(2*k2[4]))+(2*k3[4])+k4[4])/6));
		
		k1[5]=(b.angularVelocity/time);
		k2[5]=(b.angularVelocity+(step/2))/(time+(step*k1[5])/2);
		k3[5]=(b.angularVelocity+(step/2))/(time+(step*k2[5])/2);
		k4[5]=(b.angularVelocity+step)/(time+(step*k3[5]));
		b.setAngularVelocity(b.getAngularVelocity()+((step*(k1[5]+(2*k2[5]))+(2*k3[5])+k4[5])/6));
		
		if(check==bodies.size()-1){
			time++;
			check=0;
		}
		else
		{
		check++;
		}
	}
		
}


